package com.grt192.benchtest.mechanism;

import com.grt192.actuator.GRTJaguar;
import com.grt192.core.Command;
import com.grt192.core.Mechanism;
import com.grt192.mechanism.GRTDriverStation;

/**
 *
 * @author anand
 */
public class BenchMechanism extends Mechanism {

    public static final int WAIT_TIME = 5000;
    private GRTJaguar bottomJag1;
    private GRTJaguar bottomJag2;
    private GRTJaguar bottomJag3;
    private GRTJaguar bottomJag4;
    private GRTJaguar topJag1;
    private GRTJaguar topJag2;
    private GRTJaguar topJag3;
    private GRTJaguar topJag4;
    private GRTJaguar topJag5;

    public BenchMechanism() {
        bottomJag1 = new GRTJaguar(2);
        bottomJag1.start();
        bottomJag2 = new GRTJaguar(3);
        bottomJag2.start();
        bottomJag3 = new GRTJaguar(4);
        bottomJag3.start();
        bottomJag4 = new GRTJaguar(5);
        bottomJag4.start();

        topJag1 = new GRTJaguar(6);
        topJag1.start();

        topJag2 = new GRTJaguar(7);
        topJag2.start();

        topJag3 = new GRTJaguar(8);
        topJag3.start();

        topJag4 = new GRTJaguar(9);
        topJag4.start();

        topJag5 = new GRTJaguar(10);
        topJag5.start();

        addActuator("top1", topJag1);
        addActuator("top2", topJag2);
        addActuator("top3", topJag3);
        addActuator("top4", topJag4);
        addActuator("top5", topJag5);
        addActuator("bottom1", bottomJag1);
        addActuator("bottom2", bottomJag2);
        addActuator("bottom3", bottomJag3);
        addActuator("bottom4", bottomJag4);
    }

    public void testTimedPorts() {
        System.out.println("Running 1");
        bottomJag1.enqueueCommand(new Command(1.0, WAIT_TIME, true));
        try {
            Thread.sleep(WAIT_TIME);
        } catch (Exception e) {
        }
        ;
        System.out.println("Running 2");
        bottomJag2.enqueueCommand(new Command(1.0, WAIT_TIME, true));
        try {
            Thread.sleep(WAIT_TIME);
        } catch (Exception e) {
        }

        System.out.println("Running 3");
        bottomJag3.enqueueCommand(new Command(1.0, WAIT_TIME, true));
        try {
            Thread.sleep(WAIT_TIME);
        } catch (Exception e) {
        }

        System.out.println("Running 4");
        bottomJag4.enqueueCommand(new Command(1.0, WAIT_TIME, true));
        try {
            Thread.sleep(WAIT_TIME);
        } catch (Exception e) {
        }

        System.out.println("Running t1");
        topJag1.enqueueCommand(new Command(1.0, WAIT_TIME, true));
        try {
            Thread.sleep(WAIT_TIME);
        } catch (Exception e) {
        }

        System.out.println("Running t2");
        topJag2.enqueueCommand(new Command(1.0, WAIT_TIME, true));
        try {
            Thread.sleep(WAIT_TIME);
        } catch (Exception e) {
        }

        System.out.println("Running t3");
        topJag3.enqueueCommand(new Command(1.0, WAIT_TIME, true));
        try {
            Thread.sleep(WAIT_TIME);
        } catch (Exception e) {
        }

        System.out.println("Running t4");
        topJag4.enqueueCommand(new Command(1.0, WAIT_TIME, true));
        try {
            Thread.sleep(WAIT_TIME);
        } catch (Exception e) {
        }

        System.out.println("Running t5");
        topJag5.enqueueCommand(new Command(1.0, WAIT_TIME, true));
        try {
            Thread.sleep(WAIT_TIME);
        } catch (Exception e) {
        }
    }
}
